#include <rtthread.h>

#include <stdio.h>
#if 0
#include <dshot/dshot.h>

static int dshot(int argc, char **argv)
{
    uint16_t val0[4] = {0, 0, 0, 0};
    uint16_t val[4] = {200, 200, 200, 200};

    dshot_init(1200);
    rt_thread_mdelay(500);

    for (int i = 0; i < 2000; i ++)
    {
        dshot_write(val0, 4);
        rt_thread_mdelay(2);        
    }

    printf("motor init done\r\n");

    for (int i = 0; i < 50000; i ++)
    {
        dshot_write(val, 4);
        rt_thread_mdelay(2); 
    }

    return 0;
}
MSH_CMD_EXPORT(dshot, test dshot);
#endif

#include <dshot.h>

static int dshot(int argc, char **argv)
{
    uint16_t val0[4] = {0, 0, 0, 0};
    uint16_t val[4] = {200, 200, 200, 200};
    NXMFC::dshot ds;

    if (!ds.init(":dev/pwm0"))
    {
        printf("init dshot fail\n");
        return -1;
    }

    rt_thread_mdelay(500);

    for (int i = 0; i < 2000; i ++)
    {
        ds.output(val0);
        rt_thread_mdelay(2);
    }

    printf("motor init done\r\n");

    for (int i = 0; i < 1000; i ++)
    {
        ds.output(val);
        rt_thread_mdelay(2); 
    }

    return 0;
}
MSH_CMD_EXPORT(dshot, test dshot);
